from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取包的共享目录
    # pkg_share = get_package_share_directory('one_line_lidar_mapping')

    gps_kalman_filter_node = Node(
        package='robot_odom_transform',
        executable='gps_kalman_filter_node',
        name='gps_kalman_filter_node',
        output='screen',
        parameters=[{
            "lat.process_noise": 1e-7,  # 纬度的过程噪声
            "lat.measure_noise": 1e-6,  # 纬度的测量噪声
            "lat.estimate_error": 1.0,  # 纬度的估计误差
            "lat.init_value": 0.0,  # 纬度的初始值
            "lon.process_noise": 1e-7,  # 经度的过程噪声
            "lon.measure_noise": 1e-6,  # 经度的测量噪声
            "lon.estimate_error": 1.0,  # 经度的估计误差
            "lon.init_value": 0.0,  # 经度的初始值
            "alt.process_noise": 1e-4,  # 高度的过程噪声
            "alt.measure_noise": 1e-2,  # 高度的测量噪声
            "alt.estimate_error": 1.0,  # 高度的估计误差
            "alt.init_value": 0.0,  # 高度的初始值
        }],
        remappings=[
            ('/fix', '/fix'),  # 原始gps数据话题
            ('/fix/filter', '/fix/filter'),  # 滤波后的话题
        ],
    )

    gps_to_utm_odometry_node = Node(
        package='robot_odom_transform',
        executable='gps_to_utm_odometry_node',
        name='gps_to_utm_odometry_node',
        output='screen',
        parameters=[{
                "gps_topic": "/fix/filter",  # GPS话题
                "odom_topic": "utm/gps",  # 输出的UTM里程计话题
                "zone_northp_topic": "utm/zone/northp",  # UTM分区和北半球标志话题
                "frame_id": "utm",  # 输出的UTM里程计坐标系
                "child_frame_id": "gps",  # 输出的UTM里程计子坐标系
                "use_velocity": False,  # 是否进行航向推算
                "use_yaw": False,  # 是否进行速度推算
        }],
    )



    tf_odom_to_gps_node = Node(
        package='robot_odom_transform',
        executable='tf_odom_to_gps',

        name='tf_odom_to_gps',
        output='screen'
    )

    test_transform_node = Node(
        package='robot_odom_transform',
        executable='test_transform_node',

        name='test_transform_node',
        output='screen'
    )

    odom_to_gps_node = Node(
        package='robot_odom_transform',
        executable='odom_to_gps_node',

        name='odom_to_gps_node',
        output='screen',
        parameters=[{
            "fusion_odom_topic": "odom_transformed",  # 输入的里程计话题
            "initial_pose_topic": "utm/initial_pose",  # 初始定位姿态话题
            "transformed_gps_topic": "gps/fusion",  # 转换后的GPS话题
            "zone_northp_topic": "utm/zone/northp",  # utm地理分区
        }]
    )
    
    pub_dual_gnss_node = Node(
        package='robot_function',
        executable='publish_dual_gnss',
        name='publish_dual_gnss',
        output='screen',
    )
    # 返回启动描述
    return LaunchDescription([
        gps_kalman_filter_node,
        gps_to_utm_odometry_node,
        odom_to_gps_node,
        tf_odom_to_gps_node,
        test_transform_node,
        pub_dual_gnss_node
    ])
